package stuttgart.uni.pedometer;

import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;


//Calculate the Orientation of the device in geodetic coordinate system
public class OrientationCalculator implements SensorEventListener {

//Lowpass Filter Constant
private final float alpha = 0.2f;
	
	private float[] magnet;
	private float[] accel;
	
	private double roll;
	private double pitch;
	private double azimut;
	
	private SensorManager mSensorManager = null;
	
	
	public OrientationCalculator(SensorManager sm) {
    	
    	magnet = new float[3];
    	accel = new float[3];
    	
        mSensorManager = sm;
        initListeners();
	}
	
	
	public double getAzimut() {
		return azimut;
	}


	public void close() {
		mSensorManager.unregisterListener(this);
	}

	//registers Sensors
	public void initListeners() {
		mSensorManager.registerListener(this,
				mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER),
				SensorManager.SENSOR_DELAY_FASTEST);

		mSensorManager.registerListener(this,
				mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),
				SensorManager.SENSOR_DELAY_FASTEST);
	}

	/*onSensorChanged refresh sensor data after an event */
	public void onSensorChanged(SensorEvent event) {
		switch (event.sensor.getType()) {

		// here we are working with the accelerator sensor
		case Sensor.TYPE_ACCELEROMETER:
			
			//Lowpass filtering
			accel[0] = accel[0] + alpha * (event.values[0]-accel[0]);
			accel[1] = accel[1] + alpha * (event.values[1]-accel[1]);
			accel[2] = accel[2] + alpha * (event.values[2]-accel[2]);
			
			calAccMagOrientation();
			break;
			
		// here we are working with the magnetic field sensor
		case Sensor.TYPE_MAGNETIC_FIELD:
			
			//Lowpass filtering
			magnet[0] = magnet[0] + alpha * (event.values[0]-magnet[0]);
			magnet[1] = magnet[1] + alpha * (event.values[1]-magnet[1]);
			magnet[2] = magnet[2] + alpha * (event.values[2]-magnet[2]);
			
			
			break;
		}
	}
    

	/* calAccMagOrientation is used to find out the orientation of our device */
    private void calAccMagOrientation() {
   
    	float[] rotmat = new float[9];
    	float[] ori = new float[3];
    	
    	SensorManager.getRotationMatrix(rotmat, null, accel, magnet);
    	SensorManager.getOrientation(rotmat, ori);
		
    	roll = ori[2];
		pitch = ori[1];
		azimut = ori[0];
	}
    
   
	
    public void onAccuracyChanged(Sensor sensor, int accuracy) {	
	}

}
